// CaptureWrapperOpenCV
// Wrapper of OpenCV functionality for capturing webcam images
// 
// FLMScan - Featured by FabLab Munich

using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Runtime.InteropServices;
using System.Drawing.Imaging;
using FLMScan.Geometry;
using FLMScan.OpenCV;


namespace FLMScan.OpenCV
{
    internal class CaptureWrapperOpenCV
    {
        private const string HIGHGUI_LIBRARY = ".\\3rdParty\\OpenCV\\highgui210.dll";
        private const string CXCORE_LIBRARY = ".\\3rdParty\\OpenCV\\cxcore210.dll";
        private const string CV_LIBRARY = ".\\3rdParty\\OpenCV\\cv210.dll";
        
        public static CvCapture CvCreateCameraCapture(int idx)
        {
            CvCapture c = new CvCapture();
            c.ptr = cvCreateCameraCapture(idx);
            return c;
        }

        [DllImport(HIGHGUI_LIBRARY)]
        private static extern IntPtr cvCreateCameraCapture(int idx);

        public static void CvReleaseCapture(ref CvCapture capture)
        {
            cvReleaseCapture(ref capture.ptr);
        }

        [DllImport(HIGHGUI_LIBRARY)]
        private static extern void cvReleaseCapture(ref IntPtr ptr);

        public static IplImage CvQueryFrame(ref CvCapture capture)
        {
            IntPtr p = cvQueryFrame(capture.ptr);
            IplImage i;
            i = (IplImage)Marshal.PtrToStructure(p, typeof(IplImage));
            i.ptr = p;
            return i;
        }

        [DllImport(HIGHGUI_LIBRARY)]
        private static extern IntPtr cvQueryFrame(IntPtr ptr);

        public static void CvReleaseImageHeader(ref IplImage image)
        {
            cvReleaseImageHeader(ref image.ptr);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvReleaseImageHeader(ref IntPtr image);

        [DllImport("kernel32.dll", EntryPoint = "CopyMemory")]
        public static extern void memcpy(IntPtr dest, IntPtr src, int len);

        public static void CvReleaseImage(ref IplImage image)
        {
            cvReleaseImage(ref image.ptr);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvReleaseImage(ref IntPtr image);

        public static CvRect CvGetImageROI(ref IplImage image)
        {
            return cvGetImageROI(ref image);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern CvRect cvGetImageROI(ref IplImage image);

        public static Bitmap ToBitmap(IplImage image, bool dispose)
        {
            if (image.dataOrder != 0)
            {
                throw new System.Exception("Only interleaved Images are supported for conversion to Bitmap");
            }


            if (image.nChannels != 3 || image.depth != OpenCVConstants.IPL_DEPTH_8U)
            {
                throw new System.Exception("Only 3 Channel and IPL_DEPTH_8U images are supported for conversion to Bitmap");
            }


            CvSize size;
            int Width = size.width = image.width;
            int Height = size.height = image.height;

            //create the bitmap and get the pointer to the data
            Bitmap gdiBmp;
            PixelFormat fmt;
            fmt = PixelFormat.Format24bppRgb;
            gdiBmp = new Bitmap(Width, Height, fmt);

            BitmapData data = gdiBmp.LockBits(new Rectangle(0, 0, Width, Height),
                    ImageLockMode.WriteOnly, fmt);
            int dataPtr = data.Scan0.ToInt32();

            int start, elementCount, byteWidth, rows, widthStep;
            RoiParam(image, out start, out rows, out elementCount, out byteWidth, out widthStep);
            for (int row = 0; row < data.Height; row++, start += widthStep, dataPtr += data.Stride)
                memcpy((IntPtr)dataPtr, (IntPtr)start, data.Stride);

            gdiBmp.UnlockBits(data);

            if (dispose) CvReleaseImage(ref image);
            return gdiBmp;
        }

        public static IplImage ToIplImage(Bitmap img, bool dispose)
        {
            IplImage iplImage;
            BitmapData data;

            // check format (Es darf nicht das UND es darf nicht das andere sein)
            if (img.PixelFormat != PixelFormat.Format24bppRgb && img.PixelFormat != PixelFormat.Format8bppIndexed)
            {
                throw new System.Exception("Only unsigned 8/24 Bit Image Depht are supported for conversion to IplImage");
            }

            // set size
            CvSize size = new CvSize(img.Width, img.Height);

            // create
            if (img.PixelFormat == PixelFormat.Format24bppRgb)
            {
                iplImage = CvCreateImage(size, (int)OpenCVConstants.IPL_DEPTH_8U, 3);
                data = img.LockBits(new Rectangle(0, 0, size.width,
                size.height), ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
            }
            else
            {
                iplImage = CvCreateImage(size, (int)OpenCVConstants.IPL_DEPTH_8U, 1);
                data = img.LockBits(new Rectangle(0, 0, size.width,
                size.height), ImageLockMode.WriteOnly, PixelFormat.Format8bppIndexed);
            }

            // copy data
            int dataPtr = data.Scan0.ToInt32();

            int start, elementCount, byteWidth, rows, widthStep;
            RoiParam(iplImage, out start, out rows, out elementCount, out byteWidth, out widthStep);
            for (int row = 0; row < data.Height; row++, start += widthStep, dataPtr += data.Stride)
                memcpy((IntPtr)start, (IntPtr)dataPtr, data.Stride);

            img.UnlockBits(data);

            if (dispose) img.Dispose();

            return iplImage;
        }

        public static IplImage CvCreateImage(CvSize size, int depth, int channels)
        {
            IntPtr p = cvCreateImage(size, depth, channels);
            IplImage i = (IplImage)Marshal.PtrToStructure(p, typeof(IplImage));
            i.ptr = p;
            return i;
        }
        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern IntPtr cvCreateImage(CvSize size, int depth, int channels);

        [DllImport(CV_LIBRARY)]
        private static extern void cvCvtColor(ref IplImage src, ref IplImage dst, int code);        

        public static void CvGoodFeaturesToTrack(ref IplImage image, ref IplImage eig_image, ref IplImage temp_image, IntPtr corners, ref int corner_count, double quality_level, double min_distance, IntPtr mask, int block_size, int use_harris, double k)
        {
            cvGoodFeaturesToTrack(ref image, ref eig_image, ref temp_image, corners, ref corner_count, quality_level, min_distance, mask, block_size, use_harris, k);
        }

        [DllImport(CV_LIBRARY)]
        private static extern void cvGoodFeaturesToTrack(ref IplImage image, ref IplImage eig_image, ref IplImage temp_image, IntPtr corners, ref int corner_count, double quality_level, double min_distance, IntPtr mask, int block_size, int use_harris, double k);

        public static IntPtr Convert1DArrToPtr(CvPoint2D32f[] arr, out GCHandle handle)
        {
            handle = new GCHandle();
            handle = GCHandle.Alloc(arr, GCHandleType.Pinned);
            return handle.AddrOfPinnedObject();
        }

        /// <summary>
        /// Release the used handle
        /// </summary>
        /// <param name="h">Handle to be released</param>
        public static void ReleaseHandel(GCHandle h)
        {
            h.Free();
        }

        unsafe public static List<FlmPoint> PointsAboveThreshold(IplImage frame, int channel, byte threshold)
        {
            List<FlmPoint> points = new List<FlmPoint>();

            int height = frame.height;
            int width = frame.width;
            int step = frame.widthStep/sizeof(byte);
            int channels = frame.nChannels;
            byte *data = (byte *)frame.imageData;

            // channel 0: blue
            // channel 1: green
            // channel 2: red
                        

            for(int i = 0; i < height; i++)
            {
                for (int j = 0; j < width; j++)
                {
                    byte pixelValue = data[i * step + j * channels + channel];
                    if (pixelValue >= threshold)
                    {
                        points.Add(new FlmPoint(j, i));                
                    }
                }                
            }

            return points;
        }

        public static List<FlmPoint> GoodFeaturesToTrack(IplImage frame)
        {
            IplImage featuresToTrackImage;

            // check if this is the first run
            // if (featuresToTrackImage.ptr == IntPtr.Zero)
            {
                /* allocate all the buffers */
                featuresToTrackImage = CvCreateImage(CvGetSize(ref frame), 8, 3);
                featuresToTrackImage.origin = frame.origin;
            }

            CvCopy(ref frame, ref featuresToTrackImage);

            List<FlmPoint> points = new List<FlmPoint>();
            int corner_count = 1000;

            /// create gray scale image
            IplImage gray = CvCreateImage(new CvSize(featuresToTrackImage.width, featuresToTrackImage.height), (int)OpenCVConstants.IPL_DEPTH_8U, 1);
            IplImage eig_image = CvCreateImage(new CvSize(featuresToTrackImage.width, featuresToTrackImage.height), (int)OpenCVConstants.IPL_DEPTH_32F, 1);
            IplImage tmp_image = CvCreateImage(new CvSize(featuresToTrackImage.width, featuresToTrackImage.height), (int)OpenCVConstants.IPL_DEPTH_32F, 1);
            CvPoint2D32f[] pts = new CvPoint2D32f[corner_count];

            // TBD: uncomment this - it is crashing for some reason
            /// do color conversion
            CvCvtColor(ref featuresToTrackImage, ref gray, OpenCVConstants.CV_BGR2GRAY);

            GCHandle h;
            //CvGoodFeaturesToTrack(ref gray, ref eig_image, ref tmp_image, Convert1DArrToPtr(pts, out h),
            //    ref corner_count, 0.01, 1, IntPtr.Zero, 3, 1, 0.04);
            CvGoodFeaturesToTrack(ref gray, ref eig_image, ref tmp_image, Convert1DArrToPtr(pts, out h),
                ref corner_count, 0.005, 1, IntPtr.Zero, 3, 1, 0.04);

            // ref IplImage image
            // ref IplImage eig_image
            // ref IplImage temp_image
            // IntPtr corners
            // ref int corner_count
            
            // double quality_level => 0.01
            // double min_distance => 1
            // IntPtr mask => null pointer
            // int block_size => 3
            // int use_harris => 1
            // double k => 0.04

            for(int i = 0; i < corner_count; i++ )
            {
                points.Add(new FlmPoint((double)pts[i].x, (double)pts[i].y, 0.0));
            }

            CvReleaseImage(ref eig_image);
            CvReleaseImage(ref tmp_image);
            CvReleaseImage(ref gray);
            CvReleaseImage(ref featuresToTrackImage);
            ReleaseHandel(h);

            return points;
        }

        public static void CvCvtColor(ref IplImage src, ref IplImage dst, int code)
        {
            cvCvtColor(ref src, ref dst, code);
        }

        public static CvSize CvGetSize(ref IplImage arr)
        {
            return cvGetSize(ref arr);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern CvSize cvGetSize(ref IplImage arr);

        public static void CvCopy(ref IplImage src, ref IplImage dst)
        {
            cvCopy(ref src, ref dst, IntPtr.Zero);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvCopy(ref IplImage src, ref IplImage dst, IntPtr mask);

        private static void RoiParam(IplImage img, out int start, out int rows, out int elementCount, out int byteWidth, out int widthStep)
        {
            start = img.imageData.ToInt32();
            widthStep = img.widthStep;

            if (img.roi != IntPtr.Zero)
            {
                CvRect rec = CvGetImageROI(ref img);
                elementCount = rec.width * img.nChannels;
                byteWidth = (img.depth >> 3) * elementCount;

                start += rec.y * widthStep + (img.depth >> 3) * rec.x;
                rows = rec.height;
            }
            else
            {
                byteWidth = widthStep;
                elementCount = img.width * img.nChannels;
                rows = img.height;
            }
        }

        public static void MarkFeatures(ref IplImage frame, List<FlmPoint> points)
        {
            int lineThickness = 2;
            int lineType = 8;
            CvScalar circleColor = new CvScalar(0, 255, 0, 0);

            int startIndex = 0;
            int currentIndex = 0;
            if(points.Count > 1000)
                startIndex = points.Count - 1000;

            foreach (FlmPoint p in points)
            {
                if(currentIndex > startIndex)
                    CvCircle(ref frame, new CvPoint((int)p.X, (int)p.Y), 1, circleColor, lineThickness, lineType, 0);
                ++currentIndex;
            }    
        }

        public static void CvCircle(ref IplImage img, CvPoint center, int radius, CvScalar color, int thickness, int line_type, int shift)
        {
            cvCircle(ref img, center, radius, color, thickness, line_type, shift);
        }

        [DllImport(CXCORE_LIBRARY, CharSet = CharSet.Ansi)]
        private static extern void cvCircle(ref IplImage img, CvPoint center, int radius, CvScalar color, int thickness, int line_type, int shift);

        public static int CvSetCaptureProperty(ref CvCapture capture, int property_id, double value)
        {
            return cvSetCaptureProperty(capture.ptr, property_id, value);
        }
        [DllImport(HIGHGUI_LIBRARY)]
        private static extern int cvSetCaptureProperty(IntPtr ptr, int property_id, double value);                                
    }
}
